/***********************************

Function: Acc_getValues.c

************************************/
#include<math.h>
#include "User_define.h"
void Acc_Filter_Value(double *,int  );
void Acc_Values_Nomalize(double *, int );
void Acc_getFinal(double *, int );
void getLPCoefficientsButterworth2Pole(const int , const double , double* const , double* const );
void filter(double* , int );
void Gyro_getValues(double *, int );
void Euler_Transform(double *, int );
void Gyro_getAngleEuler(double *, int );
void Get_RollPitch(double *, int );

void Acc_Filter_Value(double *samples, int count)
{
 double Acc[3]={0.0,0.0,0.0};
 int i;
  Acc[0] = (((AdcRegs.ADCRESULT4>>4)*MAX_AD_VOLTAGE/ADC_SCALE)-VOLTGEZERO_X_AXIS)*5;
  Acc[1] = (((AdcRegs.ADCRESULT6>>4)*MAX_AD_VOLTAGE/ADC_SCALE)-VOLTGEZERO_Y_AXIS)*5;
  Acc[2] = (((AdcRegs.ADCRESULT5>>4)*MAX_AD_VOLTAGE/ADC_SCALE)-VOLTGEZERO_Z_AXIS)*5;
 filter(Acc,3);

 for( i=0;i<count;i++){
   samples[i]=Acc[i];}

}

void Acc_Values_Nomalize(double *Accelerometer, int count)
{
  double Acc_N[3]={0.0,0.0,0.0};
  double norm=0.0;
  int i;
  Acc_Filter_Value(Acc_N,3);
  norm=sqrt(Acc_N[0]*Acc_N[0]+Acc_N[1]*Acc_N[1]+Acc_N[2]*Acc_N[2]);
  Acc_N[0]=Acc_N[0]/norm;
  Acc_N[1]=Acc_N[1]/norm; 
  Acc_N[2]=Acc_N[2]/norm;
  for( i=0 ;i<count; i++){
    Accelerometer[i]= Acc_N[i];  }

}

void Acc_getFinal(double *Acc_angle, int count)
{
  double AccAngle[2]={0.0,0.0};
  double Acc_Final[3]={0.0, 0.0, 0.0};
  int i;
  Acc_Values_Nomalize(Acc_Final, 3);
  for( i= 0; i<count; i++)
   {
     AccAngle[0]=asin(-Acc_Final[0]);
	 AccAngle[1]=asin(Acc_Final[1]/AccAngle[0]);
	 }

}

void getLPCoefficientsButterworth2Pole(const int samplerate, const double cutoff, double* const ax, double* const by)
{
    //const double PI      = 3.1415926535897932385;
    //const double sqrt2 = 1.4142135623730950488;

    double QcRaw  = (2 * /*PI */3.1415926535897932385 * cutoff) / samplerate; // Find cutoff frequency in [0..PI]
    double QcWarp = tan(QcRaw); // Warp cutoff frequency

    double gain = 1 / (1+/*sqrt2*/1.4142135623730950488/QcWarp + 2/(QcWarp*QcWarp));
    by[2] = (1 - /*sqrt2*/ 1.4142135623730950488/QcWarp + 2/(QcWarp*QcWarp)) * gain;
    by[1] = (2 - 2 * 2/(QcWarp*QcWarp)) * gain;
    by[0] = 1;
    ax[0] = 1 * gain;
    ax[1] = 2 * gain;
    ax[2] = 1 * gain;
}

double xv[3];
double yv[3];

void filter(double* samples, int count)
{
   double ax[3];
   double by[3];
   int i;

   getLPCoefficientsButterworth2Pole(100, 80, ax, by);

   for ( i=0;i<count;i++)
   {
       xv[2] = xv[1]; xv[1] = xv[0];
       xv[0] = samples[i];
       yv[2] = yv[1]; yv[1] = yv[0];

       yv[0] =   (ax[0] * xv[0] + ax[1] * xv[1] + ax[2] * xv[2]
                    - by[1] * yv[0]
                    - by[2] * yv[1]);

       samples[i] = yv[0];
   }
}

/*******************************************************************

                                GyroScope

********************************************************************/



void Gyro_getValues(double *samples, int count)
{
 double Gyro[3]={0.0,0.0,0.0};
 int j,i;
  double gOffsets1=0.0;
  double gOffsets2=0.0;
  double gOffsets3=0.0;
   Gyro[0] =((((AdcRegs.ADCRESULT2>>4)  * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD) /( 2.0 * VDD * SCALE_FACTOR) )/ 180 * PI;//theta_dot_3 pitch
   Gyro[1] =((((AdcRegs.ADCRESULT1>>4)  * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD)/( 2.0 * VDD * SCALE_FACTOR) )/ 180 * PI ;//theta_dot_1 roll
   Gyro[2] =((((AdcRegs.ADCRESULT3>>4)  * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD)/( 2.0 * VDD * SCALE_FACTOR) )/ 180 * PI;//theta_dot_2 yaw;
   

      for( j=0;j<64;j++)
       {

	     gOffsets1=(gOffsets1*j+Gyro[0])/(j+1);
	     gOffsets2=(gOffsets2*j+Gyro[1])/(j+1);
	     gOffsets3=(gOffsets3*j+Gyro[2])/(j+1);
       }

      Gyro[0] -= gOffsets1;//pitch
	  Gyro[1] -= gOffsets2;//roll
	  Gyro[2] -= gOffsets3;//yaw 

 for( i=0;i<count;i++)
   samples[i]=Gyro[i];

}

void Euler_Transform(double *DT_Euler, int count)
 {
   double buff_Gyro[3]={0.0,0.0,0.0};
   double buff_AngleEuler[2]={0.0,0.0};
   double Rate_Angle_Euler[2]={0.0,0.0};
   int i;
   Gyro_getValues(buff_Gyro, 3);
   Get_RollPitch(buff_AngleEuler, 2);
  
   
     Rate_Angle_Euler[0]=(cos(buff_AngleEuler[1]) * cos(buff_AngleEuler[0]) * buff_Gyro[1] - sin(buff_AngleEuler[1]) * sin(buff_AngleEuler[0]) * buff_Gyro[2])/cos(buff_AngleEuler[1]);//dt of pitch euler angle
	 Rate_Angle_Euler[1]=(cos(buff_AngleEuler[0]) *  buff_Gyro[0] + sin(buff_AngleEuler[1]) * sin(buff_AngleEuler[0]) * buff_Gyro[1] + cos(buff_AngleEuler[1]) * cos(buff_AngleEuler[0]) * buff_Gyro[2])/cos(buff_AngleEuler[1]);

   

 for( i=0;i<count;i++)
  {
    DT_Euler[i]=Rate_Angle_Euler[i];
	}

}

void Gyro_getAngleEuler(double *GyroEuler, int count)
{
  double buff_AngleEuler[2]={0.0,0.0};
  double Rate_Angle[2]={0.0,0.0};
  double Angle_Gyro[2]={0.0,0.0};
  int i;
  Get_RollPitch(buff_AngleEuler,2);
  Euler_Transform(Rate_Angle, 2);
  
  Angle_Gyro[0]=buff_AngleEuler[0] + Rate_Angle[0] * STEP;
  Angle_Gyro[1]=buff_AngleEuler[1] + Rate_Angle[1] * STEP;


 for( i=0;i<count;i++)
  {
    GyroEuler[i]=Angle_Gyro[i];
	}
}

void Get_RollPitch(double *Roll_Pitch, int count)
{

 double Gyro[2]={0.0,0.0};
 double Acc[2]={0.0,0.0};
 double Angle_Robot[2]={0.0,0.0};
 int i;

 Gyro_getAngleEuler(Gyro, 2);
 Acc_getFinal(Acc, 2);


 Angle_Robot[0]=Gyro[0] * 0.98 + Acc[0] * 0.02;
 Angle_Robot[1]=Gyro[1] * 0.98 + Acc[1] * 0.02;

 for(  i=0; i<count; i++)
  {
    Roll_Pitch[i]=Angle_Robot[i];

   }

}